#include "CeresSolProjMatrix.h"

#include <vector>

#include <opencv2/core.hpp>

#include "ceres/ceres.h"
#include "gflags/gflags.h"
#include "glog/logging.h"

#include <memath/matrixd.h>

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;

using namespace std;
using namespace cv;

/**
cos(y)*cos(z)*f - sin(y)*u - r11 = 0;
(sin(x)*sin(y)*cos(z) - cos(x)*sin(z))*f + sin(x)*cos(y) - r12 = 0;
(cos(x)*sin(y)*cos(z) + sin(x)*sin(z))*f + cos(x)*cos(y) - r13 = 0;
r11=305.92  r12=-172.414   r13=467.643
x = 10 y = 20 是角度，要转成弧度
结果应该是 u=300 f=502 z=30
*/
struct F1 {
    F1(double _x, double _y, double _r11, double _r12, double _r13) {
        x = _x;
        y = _y;
        r11 = _r11;
        r12 = _r12;
        r13 = _r13;
    }
    template <typename T>
    bool operator()(const T* const u, const T* const f, const T* const z, T* residual) const {
        residual[0] = cos(y)*cos(z[0])*f[0] - sin(y)*u[0] - r11;
        return true;
    }
    double x, y;
    double r11, r12, r13;
};

struct F2 {
    F2(double _x, double _y, double _r11, double _r12, double _r13) {
        x = _x;
        y = _y;
        r11 = _r11;
        r12 = _r12;
        r13 = _r13;
    }

    template <typename T>
    bool operator()(const T* const u, const T* const f, const T* const z, T* residual) const {
        residual[0] = (sin(x)*sin(y)*cos(z[0]) - cos(x)*sin(z[0]))*f[0] + sin(x)*cos(y)*u[0] - r12;
        return true;
    }
    double x, y;
    double r11, r12, r13;
};

struct F3 {
    F3(double _x, double _y, double _r11, double _r12, double _r13) {
        x = _x;
        y = _y;
        r11 = _r11;
        r12 = _r12;
        r13 = _r13;
    }
    template <typename T>
    bool operator()(const T* const u, const T* const f, const T* const z, T* residual) const {
        residual[0] = (cos(x)*sin(y)*cos(z[0]) + sin(x)*sin(z[0]))*f[0] + cos(x)*cos(y)*u[0] - r13;
        return true;
    }
    double x, y;
    double r11, r12, r13;
};

struct F4 {
    F4(double _x, double _y, double _r21, double _r22, double _r23) {
        x = _x;
        y = _y;
        r21 = _r21;
        r22 = _r22;
        r23 = _r23;
    }
    template <typename T>
    bool operator()(const T* const v, const T* const f, const T* const z, T* residual) const {
        residual[0] = cos(y)*sin(z[0])*f[0] - sin(y)*v[0] - r21;
        return true;
    }
    double x, y;
    double r21, r22, r23;
};

struct F5 {
    F5(double _x, double _y, double _r21, double _r22, double _r23) {
        x = _x;
        y = _y;
        r21 = _r21;
        r22 = _r22;
        r23 = _r23;
    }
    template <typename T>
    bool operator()(const T* const v, const T* const f, const T* const z, T* residual) const {
        residual[0] = (sin(x)*sin(y)*sin(z[0]) + cos(x)*cos(z[0]))*f[0] + sin(x)*cos(y)*v[0] - r22;
        return true;
    }
    double x, y;
    double r21, r22, r23;
};

struct F6 {
    F6(double _x, double _y, double _r21, double _r22, double _r23) {
        x = _x;
        y = _y;
        r21 = _r21;
        r22 = _r22;
        r23 = _r23;
    }
    template <typename T>
    bool operator()(const T* const v, const T* const f, const T* const z, T* residual) const {
        residual[0] = (cos(x)*sin(y)*sin(z[0]) - sin(x)*cos(z[0]))*f[0] + cos(x)*cos(y)*v[0] - r23;
        return true;
    }
    double x, y;
    double r21, r22, r23;
};

DEFINE_string(minimizer,
              "trust_region",
              "Minimizer type to use, choices are: line_search & trust_region");


//static double x, y;
//static double r11, r12, r13;
//static double r21, r22, r23;
int CeresSolProjMatrix::solve(double x, double y,
                              double r11, double r12, double r13,
                              double r21, double r22, double r23,
                              double &u, double &v, double &f, double &z)
{
    //google::InitGoogleLogging("CeresSolProjMatrix");

    u = 1031.5;
    v = 1031.5;
    f = 12605.74770019189;
    z = 0;

    Problem problem;
    // Add residual terms to the problem using the using the autodiff
    // wrapper to get the derivatives automatically. The parameters, x, y, z, are modified in place.
    problem.AddResidualBlock(
        new AutoDiffCostFunction<F1, 1, 1, 1, 1>(new F1(x, y, r11, r12, r13)), nullptr, &u, &f, &z);
    problem.AddResidualBlock(
        new AutoDiffCostFunction<F2, 1, 1, 1, 1>(new F2(x, y, r11, r12, r13)), nullptr, &u, &f, &z);
    problem.AddResidualBlock(
        new AutoDiffCostFunction<F3, 1, 1, 1, 1>(new F3(x, y, r11, r12, r13)), nullptr, &u, &f, &z);
    problem.AddResidualBlock(
        new AutoDiffCostFunction<F4, 1, 1, 1, 1>(new F4(x, y, r21, r22, r23)), nullptr, &v, &f, &z);
    problem.AddResidualBlock(
        new AutoDiffCostFunction<F5, 1, 1, 1, 1>(new F5(x, y, r21, r22, r23)), nullptr, &v, &f, &z);
    problem.AddResidualBlock(
        new AutoDiffCostFunction<F6, 1, 1, 1, 1>(new F6(x, y, r21, r22, r23)), nullptr, &v, &f, &z);
    Solver::Options options;
    LOG_IF(
        FATAL,
        !ceres::StringToMinimizerType(FLAGS_minimizer, &options.minimizer_type))
        << "Invalid minimizer: " << FLAGS_minimizer
        << ", valid options are: trust_region and line_search.";

    options.max_num_iterations = 100;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;

    // clang-format off
//    std::cout << "Initial u = " << u
//              << ", v = " << v
//              << ", f = " << f
//              << ", z = " << z
//              << "\n";
    // clang-format on

    // Run the solver!
    Solver::Summary summary;
    Solve(options, &problem, &summary);

    // std::cout << summary.FullReport() << "\n";
    // clang-format off
//    std::cout << "Final u = " << u
//              << ", v = " << v
//              << ", f = " << f
//              << ", z = " << z * 180/PI
//              << "\n";

    // clang-format on
    return 0;
}


/// 给定一点(uv, pt), 生成矩阵A中的两个方程p1, p2
static void fillProjectMatrixA(double *p1, double *p2, Eigen::Vector3d &pt, cv::Point2f &uv)
{
    p1[0] = pt[0];
    p1[1] = pt[1];
    p1[2] = pt[2];
    p1[3] = 1;

    p1[4] = 0;
    p1[5] = 0;
    p1[6] = 0;
    p1[7] = 0;

    p1[8]  = -uv.x * pt[0];
    p1[9]  = -uv.x * pt[1];
    p1[10] = -uv.x * pt[2];
    p1[11] = -uv.x;

    p2[0] = 0;
    p2[1] = 0;
    p2[2] = 0;
    p2[3] = 0;

    p2[4] = pt[0];
    p2[5] = pt[1];
    p2[6] = pt[2];
    p2[7] = 1;

    p2[8]  = -uv.y * pt[0];
    p2[9]  = -uv.y * pt[1];
    p2[10] = -uv.y * pt[2];
    p2[11] = -uv.y;
}

bool CeresSolProjMatrix::calProjectionMatrix_UVfrW(std::vector<Eigen::Vector3d> &ptsPlane1_W, std::vector<Eigen::Vector3d> &ptsPlane2_W,
                               std::vector<cv::Point2f> &p2fPlane1_uv, std::vector<cv::Point2f> &p2fPlane2_uv,
                               Matrix4d &matrixP)
{
    if (ptsPlane1_W.size() < 4 ||  ptsPlane2_W.size() < 4 || p2fPlane1_uv.size() < 4 || p2fPlane2_uv.size() < 4) {
        return false;
    }
    Mat A = Mat(16,12,CV_64F);
    for (uint i = 0; i< 4; i++) {
        fillProjectMatrixA(A.ptr<double>(i*2), A.ptr<double>(i*2+1), ptsPlane1_W[i], p2fPlane1_uv[i]);
        fillProjectMatrixA(A.ptr<double>(i*2+8), A.ptr<double>(i*2+1+8), ptsPlane2_W[i], p2fPlane2_uv[i]);
    }
    //cout << "A:\n" << A << endl;
    Mat B = Mat(12,1,CV_64F);
    SVD::solveZ(A, B);
    double *pB = B.ptr<double>();
    double lambda = 1.0/sqrt(pB[8]*pB[8] + pB[9]*pB[9] + pB[10]*pB[10]);
    B *= lambda; // 放大10000 倍

    // cout << "B: \n" << B << endl;
    //Mat B2 = B.reshape(1, 3);
    // cout << "matrixP: \n" << B2 << endl;


    matrixP(0,0) = pB[0]; matrixP(0,1) = pB[1]; matrixP(0,2) = pB[2]; matrixP(0,3) = pB[3];
    matrixP(1,0) = pB[4]; matrixP(1,1) = pB[5]; matrixP(1,2) = pB[6]; matrixP(1,3) = pB[7];
    matrixP(2,0) = pB[8]; matrixP(2,1) = pB[9]; matrixP(2,2) = pB[10]; matrixP(2,3) = pB[11];
    matrixP(3,0) =0     ; matrixP(3,1) = 0    ; matrixP(3,2) = 0     ; matrixP(3,3) = 1;

    return true;
}


/// H * P = (px,py), 11 个参数
struct ProjMCostF1 {
    ProjMCostF1(const Eigen::Vector3d &p1, const Point2f &p2):
        pI(p1), pO(p2){}
    template <typename T>
    bool operator()(const T* const parameters,
                    T* residuals) const {
        const T& r11 = parameters[0];
        const T& r12 = parameters[1];
        const T& r13 = parameters[2];
        const T& r14 = parameters[3];

        const T& r21 = parameters[4+0];
        const T& r22 = parameters[4+1];
        const T& r23 = parameters[4+2];
        const T& r24 = parameters[4+3];

        const T& r31 = parameters[8+0];
        const T& r32 = parameters[8+1];
        T r33 = sqrt(1.0 - r31*r31 - r32*r32);
        const T& r34 = parameters[8+2];


        T px = r11*pI[0] + r12*pI[1] + r13*pI[2] + r14;
        T py = r21*pI[0] + r22*pI[1] + r23*pI[2] + r24;
        T pz = r31*pI[0] + r32*pI[1] + r33*pI[2] + r34;

        residuals[0] = px/pz - double(pO.x);
        residuals[1] = py/pz - double(pO.y);

        return true;
    }

    const Eigen::Vector3d pI;
    const Point2f pO;
};

/// H * P = (px,py), 3个参数，对应
struct ProjMCostF2 {

    template <typename T>
    bool operator()(const T* const parameters,
                    T* residuals) const {
        const T& r31 = parameters[0];
        const T& r32 = parameters[1];
        const T& r33 = parameters[2];

        residuals[0] = r31*r31 + r32*r32 + r33*r33 - 1.0;

        return true;
    }
};

/// 生成投影矩阵P  [u, v, 1]^T = P * [x, y, z, 1], 用ceres方法
bool CeresSolProjMatrix::calProjectionMatrix_UVfrW(std::vector<Eigen::Vector3d> &pts_W, std::vector<cv::Point2f> &p2f_uv,
                               Matrix4d &matrixP)
{
    // Build the problem.
    ceres::Problem problem;

    double params[11] = {1, 1, 1, 1,
                         1, 1, 1, 1,
                         0, 0, 1};

    // Set up the only cost function (also known as residual).
    for (uint i=0; i<pts_W.size();i++) {
        ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction<ProjMCostF1, 2, 11>(
                new ProjMCostF1(pts_W[i], p2f_uv[i]));
        problem.AddResidualBlock(cost_function, nullptr, params);
    }
    //problem.AddResidualBlock(new ceres::AutoDiffCostFunction<ProjMCostF2, 1, 3>(new ProjMCostF2),
    //                         nullptr, params+8);

    // Run the solver!
    ceres::Solver::Options options;
    options.minimizer_progress_to_stdout = true;
    options.gradient_tolerance = 1e-16;
    options.function_tolerance = 1e-16;
    options.parameter_tolerance = 1e-16;
    options.linear_solver_type = ceres::DENSE_QR;
    ceres::Solver::Summary summary;
    Solve(options, &problem, &summary);

    //std::cout << summary.BriefReport() << "\n";
    // std::cout << summary.FullReport() << "\n";

    matrixP(0,0) = params[0]; matrixP(0,1) = params[1]; matrixP(0,2) = params[2];  matrixP(0,3) = params[3];
    matrixP(1,0) = params[4]; matrixP(1,1) = params[5]; matrixP(1,2) = params[6];  matrixP(1,3) = params[7];
    matrixP(2,0) = params[8]; matrixP(2,1) = params[9]; matrixP(2,2) = sqrt(1-params[8]*params[8]-params[9]*params[9]); matrixP(2,3) = params[10];
    matrixP(3,0) =0         ; matrixP(3,1) = 0        ; matrixP(3,2) = 0         ; matrixP(3,3) = 1;
    return true;

}

